#include "MOTOR/motor_app.h"
#include <math.h>
#include <stdlib.h>
/**
 ****************************************************************************************************
 * DONE 用户定义区
 ****************************************************************************************************
 */

Angle_Data angle_data;
Ship_t ship;
MPU6050_Data MPU_Data;

PWM_HandleTypeDef htim_pwm_l;       // PWM 定时器句柄
PWM_HandleTypeDef htim_pwm_r;       // PWM 定时器句柄
PWM_HandleTypeDef htim_pwm_w;       // PWM 定时器句柄
PWM_HandleTypeDef htim_pwm_h;       // PWM 定时器句柄
PWM_HandleTypeDef htim_pwm_fl;      // PWM 定时器句柄
PWM_HandleTypeDef htim_pwm_fr;      // PWM 定时器句柄
PWM_HandleTypeDef htim_pwm_ftl;     // PWM 定时器句柄
PWM_HandleTypeDef htim_pwm_ftr;     // PWM 定时器句柄

I2C_HandleTypeDef hi2c_mpu;       // I2C 句柄

MOTOR_Parameter MOTOR_target_Parameter = {0};

PID_HandleTypeDef speed_pid = {0};      // 速度环 PID 参数
PID_HandleTypeDef position_pid = {0};   // 位置环 PID 参数
PID_HandleTypeDef angle_pid = {0};      // 角度环 PID 参数

//电机1
float target_position_l = 0.0f;     // 目标位置 (单位：脉冲数)
float target_speed_l = 0.0f;        // 目标速度 (单位：脉冲/秒)
//电机2
float target_position_r = 0.0f;     // 目标位置 (单位：脉冲数)
float target_speed_r = 0.0f;        // 目标速度 (单位：脉冲/秒)

float current_angle = 0.0f;       // 当前 Pitch 角度
float position_x = 0.0f;          // 船体 X 方向位置 (单位：米)
float position_y = 0.0f;          // 船体 Y 方向位置 (单位：米)

float pid_out_attitude_vel;
uint8_t attitude_stable_count = 0;

//自主巡航电机状态位
int move_starte = 0;
//自主巡航状态位
int move_started = 0;
//自主巡航时间位
int AUTO_MOVE_TIMER = 0;

SHIP_DIR AUTO_MOVE_DIR;

/**
 ****************************************************************************************************
 * DONE 初始化区
 ****************************************************************************************************
 */

/**
 * @name pwm初始化函数
 * @brief 初始化电机pwm引脚
 * @param htim_pwm；pwm句柄
 * @param htim：定时器句柄
 * @param Channel：pwm所用的通道号
 */
void Motor_PWM_Init(PWM_HandleTypeDef *htim_pwm, TIM_HandleTypeDef *htim, uint32_t Channel) 
{
    htim_pwm->htim = htim;
    htim_pwm->Channel = Channel;
    HAL_TIM_PWM_Start(htim, Channel);
    __HAL_TIM_SET_COUNTER(htim, 0);
}
/**
 * @name 电机初始化函数
 * @brief 对电机的初始化
 */
void Motor_Init(void)
{
    ship.tx = 0;
    ship.ty = 0;
    ship.px = 0;
    ship.py = 0;
    Motor_PWM_Init(&htim_pwm_l,&htim1,TIM_CHANNEL_1);
    Motor_PWM_Init(&htim_pwm_r,&htim1,TIM_CHANNEL_3);

    Motor_PWM_Init(&htim_pwm_w,&htim1,TIM_CHANNEL_2);
    Motor_PWM_Init(&htim_pwm_h,&htim1,TIM_CHANNEL_4);

    Motor_PWM_Init(&htim_pwm_fl,&htim3,TIM_CHANNEL_1);
    Motor_PWM_Init(&htim_pwm_fr,&htim3,TIM_CHANNEL_3);

    Motor_PWM_Init(&htim_pwm_ftl,&htim3,TIM_CHANNEL_2);
    Motor_PWM_Init(&htim_pwm_ftr,&htim3,TIM_CHANNEL_4);

    MPU6050_Init(&hi2c_mpu);
}

/**
 ****************************************************************************************************
 * DONE 系统函数
 ****************************************************************************************************
 */

//-----------------------------------------------	
//电机方向控制
//-----------------------------------------------	

/**
 * @name 船体方向控制
 * @brief 控制船体方向
 * @param In1：控制左电机方向，0为正转，1为反转
 * @param In2：控制右电机方向，0为正转，1为反转
 */
void Motor_Dir_LM(uint16_t IN1)
{
    GPIO_PinState AIN1 = IN1 > 0 ? GPIO_PIN_RESET : GPIO_PIN_SET;
    GPIO_PinState AIN2 = IN1 > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET;

    HAL_GPIO_WritePin(GPIOD,AIN1_Pin,AIN1);
		HAL_GPIO_WritePin(GPIOD,AIN2_Pin,AIN2);
}

void Motor_Dir_RM(uint16_t IN2)
{
    GPIO_PinState BIN1 = IN2 > 0 ? GPIO_PIN_RESET : GPIO_PIN_SET;
    GPIO_PinState BIN2 = IN2 > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET;

    HAL_GPIO_WritePin(GPIOD,BIN1_Pin,BIN1);
		HAL_GPIO_WritePin(GPIOD,BIN2_Pin,BIN2);
}

void Motor_Direction(uint16_t In1,uint16_t In2)
{
    GPIO_PinState AIN1 = In1 > 0 ? GPIO_PIN_RESET : GPIO_PIN_SET;
    GPIO_PinState AIN2 = In1 > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET;

    GPIO_PinState BIN1 = In2 > 0 ? GPIO_PIN_RESET : GPIO_PIN_SET;
    GPIO_PinState BIN2 = In2 > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET;
    
    //左电机 
    HAL_GPIO_WritePin(GPIOD,AIN1_Pin,AIN1);
		HAL_GPIO_WritePin(GPIOD,AIN2_Pin,AIN2);
    //右电机
    HAL_GPIO_WritePin(GPIOD,BIN1_Pin,BIN1);
		HAL_GPIO_WritePin(GPIOD,BIN2_Pin,BIN2);
}

void Motor_Dirfront(void)
{
    //左正转0
    HAL_GPIO_WritePin(GPIOD,AIN1_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(GPIOD,AIN2_Pin,GPIO_PIN_RESET);
    //右正转0
    HAL_GPIO_WritePin(GPIOD,BIN1_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(GPIOD,BIN2_Pin,GPIO_PIN_RESET);
}

void Motor_Dirback(void)
{
    //左反转1
    HAL_GPIO_WritePin(GPIOD,AIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOD,AIN2_Pin,GPIO_PIN_SET);
    //右反转1
    HAL_GPIO_WritePin(GPIOD,BIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOD,BIN2_Pin,GPIO_PIN_SET);
}

void Motor_Dirleft(void)
{
    //左反转1
    HAL_GPIO_WritePin(GPIOD,AIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOD,AIN2_Pin,GPIO_PIN_SET);
    //右正转0
    HAL_GPIO_WritePin(GPIOD,BIN1_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(GPIOD,BIN2_Pin,GPIO_PIN_RESET);
}

void Motor_Dirright(void)
{
    //左正转0
    HAL_GPIO_WritePin(GPIOD,AIN1_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(GPIOD,AIN2_Pin,GPIO_PIN_RESET);
    //右反转1
    HAL_GPIO_WritePin(GPIOD,BIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOD,BIN2_Pin,GPIO_PIN_SET);
}

float Ship_Normalize_Angle(float angle)
{
  while (angle < 0) angle += 2*M_PI;
  while (angle >= 2*M_PI) angle -= 2*M_PI;
  return angle;
}


void Ship_Attitude_Ring(void)
{

  float target_heading = Ship_Normalize_Angle(angle_data.Init_Heading + angle_data.Target_Heading);
  float err_heading = target_heading - angle_data.Heading;
  err_heading = Ship_Normalize_Angle(err_heading + M_PI) - M_PI;

  pid_out_attitude_vel = -PID_Base_Cale(&angle_pid, err_heading);

  if (1.0f < fabs(pid_out_attitude_vel))
  {
    if (pid_out_attitude_vel > 0) pid_out_attitude_vel = pow(pid_out_attitude_vel, 0.6);
    else pid_out_attitude_vel = -(pow(-pid_out_attitude_vel, 0.6));
    pid_out_attitude_vel *= 1.1f;
  }

  if (fabs(err_heading)*57.0f < 0.2f)
  {
    pid_out_attitude_vel = 0;
    if (attitude_stable_count <= 3) ++attitude_stable_count;
    else if (attitude_stable_count == 4)
    {
      // if (!flag_task_finished);
      // flag_task_finished = 1;
      ++attitude_stable_count;
    }
  }
  else
  {
    attitude_stable_count = 0;
  }
}


/**
 ****************************************************************************************************
 * DONE 用户函数区
 ****************************************************************************************************
 */

/**
 * @name 船体控制函数
 * @brief 控制船体运动
 * @param pwm_l：控制左电机转速
 * @param pwm_r：控制左电机转速
 * @param ship_dir：控制电机方向
 * @result 无
 */
void SHIP_RUN(uint16_t pwm_l,uint16_t pwm_r,SHIP_DIR ship_dir)
{
    uint16_t dir_l, dir_r;
    switch (ship_dir)   
    {
    case SHIP_FRONT:  dir_l = 0; dir_r = 0;					
      break;
    case SHIP_BACK: 	dir_l = 1; dir_r = 1;			
      break;
    case SHIP_LEFT: 	dir_l = 1; dir_r = 0;				
      break;
    case SHIP_RIGHT: 	dir_l = 0; dir_r = 1;			
      break;
    }

    Motor_Direction(dir_l,dir_r);	  
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,abs(pwm_l));
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,abs(pwm_r));
}

/**
 * @name 左电机控制函数
 * @brief 控制左电机
 * @param pwm：控制电机转速
 * @param motor_dir：控制电机方向
 * @result 无
 */
void SHIP_RUN_LM(uint16_t pwm,MOTOR_DIR motor_dir)
{
    uint16_t dir;
    switch (motor_dir)
    {
    case MOTOR_foreward: dir = 0;
      break;
    case MOTOR_reversal: dir = 1;
      break;
    }
    Motor_Dir_LM(dir);
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,abs(pwm));
}

/**
 * @name 右电机控制函数
 * @brief 控制右电机
 * @param pwm：控制电机转速
 * @param motor_dir：控制电机方向
 * @result 无
 */
void SHIP_RUN_RM(uint16_t pwm,MOTOR_DIR motor_dir)
{
    uint16_t dir;
    switch (motor_dir)
    {
    case MOTOR_foreward: dir = 0;
      break;
    case MOTOR_reversal: dir = 1;
      break;
    }
    Motor_Dir_RM(dir);
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,abs(pwm));
}

/**
 * @name 采集控制函数
 * @brief 控制采集水样
 * @param pwm：控制电机转速
 * @param dir：控制电机方向，0为抽水，1为放水
 * @result 无
 */
void Motor_Get_water_sample(uint16_t pwm,uint16_t dir)
{
    if(dir)
    {
        HAL_GPIO_WritePin(GPIOD,CIN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOD,CIN2_Pin,GPIO_PIN_RESET);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOD,CIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOD,CIN2_Pin,GPIO_PIN_SET);      
    }
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,abs(pwm));
}

/**
 * @name 绕绳控制函数
 * @brief 控制绕绳电机
 * @param pwm：控制电机转速
 * @param dir：控制电机方向，1为绕绳，0为放绳
 * @result 无
 */
void Motor_Get_water_depth(uint16_t pwm,uint16_t dir)
{
    if(dir)
    {
        HAL_GPIO_WritePin(GPIOD,DIN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOD,DIN2_Pin,GPIO_PIN_RESET);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOD,DIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOD,DIN2_Pin,GPIO_PIN_SET);
    }
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,abs(pwm));
}

/**
 * @name 电机停止函数
 * @brief 控制电机停止
 * @param stop_number：控制不同电机的参数，0为检测用电机，1为运动用电机
 * @retval 无
 */
void Motor_stop(uint16_t stop_number)
{
  if(stop_number)
  {
    //运动用电机停止
    HAL_GPIO_WritePin(GPIOD,AIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOD,AIN2_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOD,BIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOD,BIN2_Pin,GPIO_PIN_RESET);
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,0);
	  __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,0);
  }
  else
  {
    //检测用电机停止
    HAL_GPIO_WritePin(GPIOD,CIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOD,CIN2_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOD,DIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOD,DIN2_Pin,GPIO_PIN_RESET);
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,0);
	  __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,0);
  }

}

/**
 * @name 船体距离控制函数 
 * @brief 控制船体运动指定距离（非阻塞式）
 * @param pwm：控制电机转速
 * @param ship_dir：控制运动方向
 * @param distance：目标距离（单位需要根据实际定义）
 * @param ship_state：船体状态结构体指针，用于保存运动状态
 * @result 返回是否完成运动
 */
uint8_t SHIP_MOVE_DISTANCE(uint16_t pwm_l,uint16_t pwm_r, SHIP_DIR ship_dir, float distance, ShipMoveState* ship_state) 
{
    // static uint32_t encoder_count_l = 0;
    // static uint32_t encoder_count_r = 0;
    
    // 如果是第一次调用，初始化运动
    if (!ship_state->is_moving) 
    {
        // 计算所需时间（假设distance单位是米，pwm对应速度m/s）
        // 需要根据实际电机速度和距离关系调整这个公式
        ship_state->required_time = (uint32_t)(distance * 1000 / (pwm_l * 0.001)); // 假设0.001是pwm到速度的转换系数 
        
        // 保存开始时间
        ship_state->start_time = HAL_GetTick();
        ship_state->is_moving = 1;
        
        // 启动电机
        SHIP_RUN(pwm_l,pwm_r, ship_dir);
        
        return 0; // 运动未完成 
    }
    else 
    {
        // 检查是否已经运动了足够时间 
        if ((HAL_GetTick() - ship_state->start_time) >= ship_state->required_time) 
        {
            // 停止电机 
            SHIP_RUN(0,0, ship_dir);
            ship_state->is_moving = 0;
            return 1; // 运动完成
        }
        
        return 0; // 运动未完成 
    }
}

void Ship_position_Ring(void)
{

}

void Ship_Move_PositionMode(Ship_t *ship, float x, float y, uint16_t speed, uint8_t acc)
{
    float Wa = (y - x) / (POS_SALE);
    float Wb = (y + x) / (POS_SALE);


}

void Ship_Move_SpeedMode(Ship_t *ship, uint16_t speed, uint8_t acc)
{

}

